//以下是定义蓝牙
const int pinRX=0;//发送口
const int pinTX=1;//接受口
//以下是陀螺仪
#include "Kalman.h"
#include <Wire.h>
#include <Math.h>
#include <avr/wdt.h>              //看门狗
float fDeg=57.295779513f;
int AA[7];
const int Time=1000;
unsigned long LastTime = 0;
float LastRoll = 0.0f;
float LastPitch = 0.0f;
Kalman kalmanRoll;
Kalman kalmanPitch;
int i=7;


void setup() {
  Serial.begin(9600); //初始化串口，指定波特率
  Wire.begin(); //初始化Wire库
  WriteMPUReg0(0x6B, 0); //一个函数：启动MPU6050设备
  //Calibration1(); //一个函数：执行校准
  LastTime = micros(); //记录当前时间
  wdt_enable(WDTO_2S); //开启看门狗，并设置溢出时间为两秒
  pinMode(i,OUTPUT);
}
void loop() {
  int BB[7];
  ReadAccGyr2(BB);//一个函数：读出角度和加速度给BB
  float CC[7];
  Rectify3(BB, CC);//一个函数：计算数据(除以相应的数)把BB里的数据给CC
  float fNorm = sqrt(CC[0] * CC[0] + CC[1] * CC[1] + CC[2] * CC[2]);//计算加速度向量的模长，均以g为单位
  float Roll = GetRoll4(CC, fNorm); //一个函数:计算Roll角
  float Pitch = GetPitch5(CC, fNorm); //一个函数:计算Pitch角
   if (CC[1] > 0) {                 //变换符号
    Roll = -Roll;
  }
  if (CC[0] < 0) {                    //变换符号·
    Pitch = -Pitch;
  }
  unsigned long CurTime = micros();      
  float dt = (double)(CurTime - LastTime) / 1000000.0;
  //对Roll角和Pitch角进行卡尔曼滤波
  float NewRoll = kalmanRoll.getAngle(Roll, CC[4], dt);
  float NewPitch = kalmanPitch.getAngle(Pitch, CC[5], dt);

  LastTime = CurTime;
  CC[2]=int(CC[2]);
  NewRoll=int(NewRoll);

  Serial.print("大腿绕x轴转动角度");Serial.print(NewRoll); Serial.print("大腿z轴上的重力加速度");Serial.print(CC[2]);
}
void WriteMPUReg0(int nReg, unsigned char nVal) {
  Wire.beginTransmission(0x68);
  Wire.write(nReg);
  Wire.write(nVal);
  Wire.endTransmission(true);
}
void WriteMPUReg01(int nReg, unsigned char nVal) {
  Wire.beginTransmission(0x69);
  Wire.write(nReg);
  Wire.write(nVal);
  Wire.endTransmission(true);
}
void Calibration1()
{
  float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
  //先求和
  for (int i = 0; i < Time; ++i) {
    int mpuVals[7];
    ReadAccGyr2(mpuVals);
    for (int j = 0; j < 7; ++j) {
      valSums[j] += mpuVals[j];
    }
  }
  //再求平均
  for (int i = 0; i < 7; ++i) {
    AA[i] = int(valSums[i] / Time);
  }
  AA[2] += 16384; //设芯片Z轴竖直向下，设定静态工作点。
}
void ReadAccGyr2(int *pVals) {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.requestFrom(0x68, 7 * 2, true);
  Wire.endTransmission(true);
  for (long i = 0; i < 7; ++i)
  {pVals[i] = Wire.read() << 8 | Wire.read();}
}

void Rectify3(int *pReadout, float *pRealVals) {
  for (int i = 0; i < 3; ++i) 
  {pRealVals[i] = (float)(pReadout[i] - AA[i]) / 16384.0f;}
  pRealVals[3] = pReadout[3] / 340.0f + 36.53;
  for (int i = 4; i < 7; ++i) 
  { pRealVals[i] = (float)(pReadout[i] - AA[i]) / 131.0f;}
}

float GetPitch5(float *pRealVals, float fNorm) {
  float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormYZ / fNorm;
  return acos(fCos) * fDeg;
}
float GetRoll4(float *pRealVals, float fNorm) {
  float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
  float fCos = fNormXZ / fNorm;
  return acos(fCos) * fDeg;
}
unsigned char ReadMPUReg(int nReg) {
  Wire.beginTransmission(0x68);
  Wire.write(nReg);
  Wire.requestFrom(0x68, 1, true);
  Wire.endTransmission(true);
  return Wire.read();
}
